#include <Average.h>

#define SONAR_PIN 2
Average* valueAverage;
Average* maxAverage;
int maxDistance;

void setup() {
  Serial.begin(9600);
  pinMode(SONAR_PIN, INPUT);
  maxAverage = newAverageWithSampleSize(500);
  valueAverage = newAverageWithSampleSize(10);
  //calibrateMaxDistance();
  delay(1000);
}

void calibrateMaxDistance() {
  delay(1000);
  for (int i = 0; i < 5; i++) {
    maxAverage->addValue(readSonar());
    delay(120);
  }
  maxDistance = maxAverage->average;
}

int readSonar() {
  long pulse = pulseIn(SONAR_PIN, HIGH);
  int inches = pulse/147;
  return inches;
}

void loop() {
  int value = readSonar();
  int ave = valueAverage->averageWithValue(value);
  ave = constrain(ave, 10, maxDistance);
  Serial.print("maxDistance: ");
  Serial.print(maxDistance);
  Serial.print(" value: ");
  Serial.print(value);
  Serial.print(" ave: ");
  Serial.print(ave);
  Serial.println(" ");
  delay(120);
}
